#!/bin/sh
# PX4 commands need the 'px4-' prefix in bash.
# (px4-alias.sh is expected to be in the PATH)
. px4-alias.sh

param select /data/px4/param/parameters

# Make sure we are running at 800Hz on IMU
param set IMU_GYRO_RATEMAX 800

# EKF2 Parameters
param set EKF2_IMU_POS_X 0.027
param set EKF2_IMU_POS_Y 0.009
param set EKF2_IMU_POS_Z -0.019
param set EKF2_EV_CTRL 15
param set EKF2_EV_DELAY 5
param set EKF2_GPS_CTRL 0
param set EKF2_ABL_LIM 0.8
param set EKF2_TAU_POS 0.25
param set EKF2_TAU_VEL 0.25

param set MC_AIRMODE 0

param set MC_YAW_P 2.0
param set MC_YAWRATE_P 0.15
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_K 1.0

param set MC_PITCH_P 5.5
param set MC_PITCHRATE_P 0.08
param set MC_PITCHRATE_I 0.2
param set MC_PITCHRATE_D 0.0013
param set MC_PITCHRATE_K 1.0

param set MC_ROLL_P 5.5
param set MC_ROLLRATE_P 0.08
param set MC_ROLLRATE_I 0.2
param set MC_ROLLRATE_D 0.0013
param set MC_ROLLRATE_K 1.0

param set MPC_VELD_LP 5.0

# tweak MPC_THR_MIN to prevent roll/pitch losing control
# authority under rapid downward acceleration
param set MPC_THR_MAX 0.75
param set MPC_THR_MIN 0.08
param set MPC_THR_HOVER 0.42
param set MPC_MANTHR_MIN 0.05

# default position mode with a little expo, smooth mode is terrible
param set MPC_POS_MODE 0
param set MPC_YAW_EXPO 0.20
param set MPC_XY_MAN_EXPO 0.20
param set MPC_Z_MAN_EXPO 0.20

# max velocities
param set MPC_VEL_MANUAL 5.0
param set MPC_XY_VEL_MAX 5.0
param set MPC_XY_CRUISE 5.0
param set MPC_Z_VEL_MAX_DN 1.5
param set MPC_Z_VEL_MAX_UP 4.0
param set MPC_LAND_SPEED 1.0

# Horizontal position PID
param set MPC_XY_P 0.95
param set MPC_XY_VEL_P_ACC 3.00
param set MPC_XY_VEL_I_ACC 0.10
param set MPC_XY_VEL_D_ACC 0.00

# Vertical position PID
# PX4 Defaults
param set MPC_Z_P 1.0
param set MPC_Z_VEL_P_ACC 8.0
param set MPC_Z_VEL_I_ACC 2.0
param set MPC_Z_VEL_D_ACC 0.0

param set MPC_TKO_RAMP_T 1.50
param set MPC_TKO_SPEED	1.50

# Set the ESC outputs to function as motors
param set MODAL_IO_FUNC1 101
param set MODAL_IO_FUNC2 103
param set MODAL_IO_FUNC3 104
param set MODAL_IO_FUNC4 102

param set MODAL_IO_SDIR1 0
param set MODAL_IO_SDIR2 0
param set MODAL_IO_SDIR3 0
param set MODAL_IO_SDIR4 0

param set MODAL_IO_CONFIG 1
param set MODAL_IO_REV 0
param set MODAL_IO_MODE 0
param set MODAL_IO_BAUD 2000000
param set MODAL_IO_RPM_MAX 10500
param set MODAL_IO_RPM_MIN 1000

# Some parameters for control allocation
param set CA_ROTOR_COUNT 4
param set CA_R_REV 0
param set CA_AIRFRAME 0
param set CA_ROTOR_COUNT 4
param set CA_ROTOR0_PX 0.15
param set CA_ROTOR0_PY 0.15
param set CA_ROTOR1_PX -0.15
param set CA_ROTOR1_PY -0.15
param set CA_ROTOR2_PX 0.15
param set CA_ROTOR2_PY -0.15
param set CA_ROTOR2_KM -0.05
param set CA_ROTOR3_PX -0.15
param set CA_ROTOR3_PY 0.15
param set CA_ROTOR3_KM -0.05

# Some parameter settings to disable / ignore certain preflight checks

# No GPS driver yet so disable it
param set SYS_HAS_GPS 0

# Allow arming wihtout a magnetometer (Need magnetometer driver)
param set SYS_HAS_MAG 0
param set EKF2_MAG_TYPE	5

# Allow arming without battery check (Need voxlpm driver)
param set CBRK_SUPPLY_CHK 894281

# Allow arming without an SD card
param set COM_ARM_SDCARD 0

# Allow arming wihtout CPU load information
param set COM_CPU_MAX 0.0

# Disable auto disarm. This is number of seconds to wait for takeoff
# after arming. If no takeoff happens then it will disarm. A negative
# value disables this.
param set COM_DISARM_PRFLT -1

# This parameter doesn't exist anymore. Need to see what the new method is.
# param set MAV_BROADCAST 0

# Doesn't work without setting this to Quadcopter
param set MAV_TYPE 2

# Parameters that we don't use but QGC complains about if they aren't there
param set SYS_AUTOSTART 4001

# Default RC channel mappings
param set RC_MAP_ACRO_SW 0
param set RC_MAP_ARM_SW 0
param set RC_MAP_AUX1 0
param set RC_MAP_AUX2 0
param set RC_MAP_AUX3 0
param set RC_MAP_AUX4 0
param set RC_MAP_AUX5 0
param set RC_MAP_AUX6 0
param set RC_MAP_FAILSAFE 0
param set RC_MAP_FLAPS 0
param set RC_MAP_FLTMODE 6
param set RC_MAP_GEAR_SW 0
param set RC_MAP_KILL_SW 7
param set RC_MAP_LOITER_SW 0
param set RC_MAP_MAN_SW 0
param set RC_MAP_MODE_SW 0
param set RC_MAP_OFFB_SW 0
param set RC_MAP_PARAM1 0
param set RC_MAP_PARAM2 0
param set RC_MAP_PARAM3 0
param set RC_MAP_PITCH 2
param set RC_MAP_POSCTL_SW 0
param set RC_MAP_RATT_SW 0
param set RC_MAP_RETURN_SW 0
param set RC_MAP_ROLL 1
param set RC_MAP_STAB_SW 0
param set RC_MAP_THROTTLE 3
param set RC_MAP_TRANS_SW 0
param set RC_MAP_YAW 4

param save

sleep 2

# Need px4-shutdown otherwise Linux system shutdown is called
px4-shutdown
